﻿using System;
using System.Collections.Generic;
using System.Linq;
using Microsoft.Xna.Framework;

namespace tdgame {
	public class Pathfinder {

		public enum MovementRange {
			Ground,
			GroundCrawler,
			Hover,
			AirHover,
			Air,
			WaterSurface,
			WaterSubmerged
		}

		struct Pt {
			public float Height;
			public double DistanceFromStart;
			public float Slope;
			public double DistanceToEnd;
		}

		struct MoveDir {
			public int X;
			public int Y;
			public double Distance;
			public MoveDir(int x, int y, double dist) {
				X = x;
				Y = y;
				Distance = dist;
			}
		}

		struct DiffPt {
			public double
				Ground,
				GroundCrawler,
				Hover,
				AirHover,
				Air,
				WaterSurface,
				WaterSubmerged;
		}

		const int initialDist = 100000;
		public double DistanceWeight = 1.0,
			DifficultyWeight = 1.0;

		Pt[,] field;
		DiffPt[,] diffField;
		public Point EndPoint;
		public Point EndPointSector;

		MoveDir[] movePoints = new MoveDir[]{
				new MoveDir(-1,0, 1),
				new MoveDir(1,0, 1),
				new MoveDir(0,-1, 1),
				new MoveDir(0,1, 1),
				new MoveDir(-1,-1, Math.Sqrt(2)),
				new MoveDir(1,-1, Math.Sqrt(2)),
				new MoveDir(-1,1, Math.Sqrt(2)),
				new MoveDir(1,1, Math.Sqrt(2))
			};

		WorldBuilder.Cell[,] CellMap;

		public Pathfinder(ref WorldBuilder.Cell[,] cellMap, Point endPt) {
			EndPoint = endPt;
			int half = cellMap.GetUpperBound(0) / 2;
			EndPointSector = new Point((endPt.X / half), (endPt.Y / half));
			CellMap = cellMap;
			field = new Pt[cellMap.GetUpperBound(0) + 1, cellMap.GetUpperBound(1) + 1];
			diffField = new DiffPt[cellMap.GetUpperBound(0) + 1, cellMap.GetUpperBound(1) + 1];

			for (int x = 0; x <= cellMap.GetUpperBound(0); x++) {
				for (int y = 0; y <= cellMap.GetUpperBound(1); y++) {
					field[x, y].Height = cellMap[x, y].BaseHeight;
					field[x, y].DistanceToEnd = MathExtras.Calculate2DDistance(new Point(x, y), endPt) * 1.0;
				}
			}

		}

		//public void ComputeDifficultyMap() {
		//    int max = field.GetUpperBound(0);
		//    int x, y, slopeWeight = 1;

		//    //if (movementType == MovementRange.Hover) slopeWeight = 3;
		//    //if (movementType == MovementRange.Ground) slopeWeight = 2;
		//    //if (movementType == MovementRange.Air) slopeWeight = 0;
		//    //if (movementType == MovementRange.AirHover) slopeWeight = 1;
		//    //if (movementType == MovementRange.WaterSubmerged || movementType == MovementRange.WaterSurface) slopeWeight = 0;

		//    for (x = 0; x <= field.GetUpperBound(0); x++) {
		//        for (y = 0; y <= field.GetUpperBound(1); y++) {
		//            diffField[x, y].Air = initialDist;
		//            diffField[x, y].AirHover = initialDist;
		//            diffField[x, y].Ground = initialDist;
		//            diffField[x, y].GroundCrawler = initialDist;
		//            diffField[x, y].Hover = initialDist;
		//            diffField[x, y].WaterSubmerged = initialDist;
		//            diffField[x, y].WaterSurface = initialDist;

		//            //field[x, y].DistanceFromStart = initialDist;
		//            ////field[x, y].DistanceToEnd = MathExtras.Calculate2DDistance(new Point(x, y), endPt) * 1.3;

		//            //if (movementType == MovementRange.AirHover)
		//            //    field[x, y].Slope = (CellMap[x, y].IsWater ? 0 : (CellMap[x, y].SlopeHeight <= 2 ? 0 : 1));
		//            //else if (movementType == MovementRange.Hover && CellMap[x, y].IsWater) {
		//            //    field[x, y].Slope = 0;
		//            //}
		//            //else
		//            //    field[x, y].Slope = CellMap[x, y].SlopeHeight * slopeWeight;

		//        }
		//    }
		//    field[startPt.X, startPt.Y].DistanceFromStart = 0;
		//    int maxIterations = max * max, iterations = 0;

		//    List<Point> scanPoints = new List<Point>();
		//    List<Vector3> retPath = new List<Vector3>();

		//    scanPoints.Add(startPt);

		//    bool searching = true;
		//    Point newPt = new Point();
		//    Point pt = endPt;
		//    double dist = initialDist;



		//    while (scanPoints.Count > 0 && searching) {
		//        x = scanPoints[0].X;
		//        y = scanPoints[0].Y;

		//        foreach (MoveDir p in ValidMoves(x, y, movementType)) {
		//            double d = field[x, y].DistanceFromStart
		//                + field[p.X, p.Y].Slope
		//                + p.Distance
		//                //+ 1
		//                ;
		//            if (d < field[p.X, p.Y].DistanceFromStart) {

		//                field[p.X, p.Y].DistanceFromStart = d;

		//                if (field[p.X, p.Y].DistanceToEnd * 5 <= targetRange) {
		//                    endPt = new Point(p.X, p.Y);
		//                }

		//                if (p.X == endPt.X && p.Y == endPt.Y) {
		//                    searching = false;
		//                    break;
		//                }
		//                if (!scanPoints.Contains(new Point(p.X, p.Y)))
		//                    scanPoints.Add(new Point(p.X, p.Y));
		//            }
		//        }
		//        scanPoints.RemoveAt(0);

		//        scanPoints = (from Point p in scanPoints
		//                      orderby field[p.X, p.Y].DistanceFromStart + field[p.X, p.Y].DistanceToEnd
		//                      select p).ToList();
		//        //if (iterations == maxIterations)
		//        //	searching = false;
		//        //iterations++;
		//    }
		//}


		public List<Vector3> ComputeBestPath(Point startPt, Point endPt, MovementRange movementType, int targetRange) {
			int max = field.GetUpperBound(0);
			int x, y, slopeWeight = 1;

			if (movementType == MovementRange.Hover) slopeWeight = 3;
			if (movementType == MovementRange.Ground) slopeWeight = 2;
			if (movementType == MovementRange.Air) slopeWeight = 0;
			if (movementType == MovementRange.AirHover) slopeWeight = 1;
			if (movementType == MovementRange.WaterSubmerged || movementType == MovementRange.WaterSurface) slopeWeight = 0;

			for (x = 0; x <= field.GetUpperBound(0); x++) {
				for (y = 0; y <= field.GetUpperBound(1); y++) {
					field[x, y].DistanceFromStart = initialDist;
					//field[x, y].DistanceToEnd = MathExtras.Calculate2DDistance(new Point(x, y), endPt) * 1.3;

					if (movementType == MovementRange.AirHover)
						field[x, y].Slope = (CellMap[x, y].IsWater ? 0 : (CellMap[x, y].SlopeHeight <= 2 ? 0 : 1));
					else if (movementType == MovementRange.Hover && CellMap[x, y].IsWater) {
						field[x, y].Slope = 0;
					}
					else
						field[x, y].Slope = CellMap[x, y].SlopeHeight * slopeWeight;

				}
			}
			field[startPt.X, startPt.Y].DistanceFromStart = 0;
			int maxIterations = max * max, iterations = 0;

			List<Point> scanPoints = new List<Point>();
			List<Vector3> retPath = new List<Vector3>();

			scanPoints.Add(startPt);

			bool searching = true;
			Point newPt = new Point();
			Point pt = endPt;
			double dist = initialDist;



			while (scanPoints.Count > 0 && searching) {
				x = scanPoints[0].X;
				y = scanPoints[0].Y;

				foreach (MoveDir p in ValidMoves(x, y, movementType)) {
					double d = field[x, y].DistanceFromStart
						+ field[p.X, p.Y].Slope
						+ p.Distance
						//+ 1
						;
					if (d < field[p.X, p.Y].DistanceFromStart) {

						field[p.X, p.Y].DistanceFromStart = d;

						if (field[p.X, p.Y].DistanceToEnd * 5 <= targetRange) {
							endPt = new Point(p.X, p.Y);
						}

						if (p.X == endPt.X && p.Y == endPt.Y) {
							searching = false;
							break;
						}
						if (!scanPoints.Contains(new Point(p.X, p.Y)))
							scanPoints.Add(new Point(p.X, p.Y));
					}
				}
				scanPoints.RemoveAt(0);

				scanPoints = (from Point p in scanPoints
							  orderby (field[p.X, p.Y].DistanceFromStart * DifficultyWeight) + (field[p.X, p.Y].DistanceToEnd * DistanceWeight)
							  select p).ToList();
				//if (iterations == maxIterations)
				//	searching = false;
				//iterations++;
			}

			if (field[endPt.X, endPt.Y].DistanceFromStart == initialDist)
				retPath.Clear();
			else {
				pt = endPt;
				int moveTurns = 0;
				searching = true;

				while (searching) {
					float ht = field[pt.X, pt.Y].Height + CellMap[pt.X, pt.Y].SlopeHeight;
					if (movementType == MovementRange.AirHover) {
						if (ht <= 100) ht = 130;
						else ht += 30;
					}
					if (movementType == MovementRange.Air) ht = 210;
					if (movementType == MovementRange.Hover && ht < 100) ht = 100;
					if (movementType == MovementRange.WaterSubmerged) ht = 100;
					if (movementType == MovementRange.WaterSurface) ht = 100;

					retPath.Insert(0, new Vector3(pt.X, ht, pt.Y));
					dist = initialDist;
					moveTurns++;

					foreach (MoveDir p in ValidMoves(pt.X, pt.Y, movementType)) {
						if (field[p.X, p.Y].DistanceFromStart < dist) {
							newPt = new Point(p.X, p.Y);
							dist = field[p.X, p.Y].DistanceFromStart;
						}
					}
					pt = newPt;
					if (newPt == startPt) {
						ht = field[startPt.X, startPt.Y].Height + CellMap[startPt.X, startPt.Y].SlopeHeight;
						if (movementType == MovementRange.AirHover) {
							if (ht <= 100) ht = 130;
							else ht += 30;
						}
						if (movementType == MovementRange.Air) ht = 210;
						if (movementType == MovementRange.Hover && ht < 100) ht = 100;
						if (movementType == MovementRange.WaterSubmerged) ht = 100;
						if (movementType == MovementRange.WaterSurface) ht = 100;

						retPath.Insert(0, new Vector3(startPt.X, ht, startPt.Y));

						searching = false;

					}
				}
			}

			return retPath;
		}

		IEnumerable<MoveDir> ValidMoves(int x, int y, MovementRange moveRange) {
			foreach (MoveDir pt in movePoints) {
				MoveDir p = new MoveDir(x + pt.X, y + pt.Y, pt.Distance);
				if (p.X >= 0 && p.X <= field.GetUpperBound(0) && p.Y >= 0 && p.Y <= field.GetUpperBound(1)) {
					bool valid = CellMap[p.X, p.Y].Traversable(moveRange);
					if (valid) {

						if (x != p.X && y != p.Y) {
							if (!CellMap[x, p.Y].Traversable(moveRange) || !CellMap[p.X, y].Traversable(moveRange))
								valid = false;
						}

						if (valid)
							yield return p;
					}
				}
			}
		}

	}
}
